package mathx

type Float interface {
	~float64 | ~float32
}

const EPS = 0.000001

type Quaternion = QuaternionT[float32]
type QuaternionD = QuaternionT[float64]

// QuaternionT[T] is QuaternionT[T] with X,Y,Z and W components.
type QuaternionT[T Float] struct {
	X T
	Y T
	Z T
	W T
}

// SetX sets this QuaternionT[T]'s X component.
// Returns pointer to this updated QuaternionT[T].
func (q *QuaternionT[T]) SetX(val T) {
	q.X = val
}

// SetY sets this QuaternionT[T]'s Y component.
// Returns pointer to this updated QuaternionT[T].
func (q *QuaternionT[T]) SetY(val T) {
	q.Y = val
}

// SetZ sets this QuaternionT[T]'s Z component.
// Returns pointer to this updated QuaternionT[T].
func (q *QuaternionT[T]) SetZ(val T) {

	q.Z = val
}

// SetW sets this QuaternionT[T]'s W component.
// Returns pointer to this updated QuaternionT[T].
func (q *QuaternionT[T]) SetW(val T) {

	q.W = val
}

// Set sets this QuaternionT[T]'s components.
// Returns pointer to this updated QuaternionT[T].
func (q *QuaternionT[T]) Set(x, y, z, w T) {

	q.X = x
	q.Y = y
	q.Z = z
	q.W = w
}

// SetIdentity sets this quanternion to the identity QuaternionT[T].
// Returns pointer to this updated QuaternionT[T].
func (q *QuaternionT[T]) SetIdentity() {

	q.X = 0
	q.Y = 0
	q.Z = 0
	q.W = 1
}

// IsIdentity returns it this is an identity QuaternionT[T].
func (q *QuaternionT[T]) IsIdentity() bool {

	if q.X == 0 && q.Y == 0 && q.Z == 0 && q.W == 1 {
		return true
	}
	return false
}

func (q *QuaternionT[T]) CopyFrom(other *QuaternionT[T]) {

	*q = *other
}

// SetFromEuler sets this QuaternionT[T] from the specified vector with
// euler angles for each axis. It is assumed that the Euler angles
// are in XYZ order.
// Returns pointer to this updated QuaternionT[T].
func (q *QuaternionT[T]) SetFromEuler(euler *Vec3T[T]) {

	c1 := Cos(euler.X / 2)
	c2 := Cos(euler.Y / 2)
	c3 := Cos(euler.Z / 2)
	s1 := Sin(euler.X / 2)
	s2 := Sin(euler.Y / 2)
	s3 := Sin(euler.Z / 2)

	q.X = s1*c2*c3 - c1*s2*s3
	q.Y = c1*s2*c3 + s1*c2*s3
	q.Z = c1*c2*s3 - s1*s2*c3
	q.W = c1*c2*c3 + s1*s2*s3

}

// SetFromAxisAngle sets this QuaternionT[T] with the rotation
// specified by the given axis and angle.
// Returns pointer to this updated QuaternionT[T].
func (q *QuaternionT[T]) SetFromAxisAngle(axis *Vec3T[T], angle T) {

	halfAngle := angle / 2
	s := Sin(halfAngle)
	q.X = axis.X * s
	q.Y = axis.Y * s
	q.Z = axis.Z * s
	q.W = Cos(halfAngle)
}

// SetFromRotationMatrix sets this QuaternionT[T] from the specified rotation matrix.
// Returns pointer to this updated QuaternionT[T].
func (q *QuaternionT[T]) SetFromRotationMatrix(m Mat44T[T]) {

	m11 := m[0]
	m12 := m[4]
	m13 := m[8]
	m21 := m[1]
	m22 := m[5]
	m23 := m[9]
	m31 := m[2]
	m32 := m[6]
	m33 := m[10]
	trace := m11 + m22 + m33

	var s T
	if trace > 0 {
		s = T(0.5 / Sqrt(float64(trace+1.0)))
		q.W = T(0.25) / s
		q.X = (m32 - m23) * s
		q.Y = (m13 - m31) * s
		q.Z = (m21 - m12) * s
	} else if m11 > m22 && m11 > m33 {
		s = 2.0 * Sqrt(1.0+m11-m22-m33)
		q.W = (m32 - m23) / s
		q.X = 0.25 * s
		q.Y = (m12 + m21) / s
		q.Z = (m13 + m31) / s
	} else if m22 > m33 {
		s = 2.0 * Sqrt(1.0+m22-m11-m33)
		q.W = (m13 - m31) / s
		q.X = (m12 + m21) / s
		q.Y = 0.25 * s
		q.Z = (m23 + m32) / s
	} else {
		s = 2.0 * Sqrt(1.0+m33-m11-m22)
		q.W = (m21 - m12) / s
		q.X = (m13 + m31) / s
		q.Y = (m23 + m32) / s
		q.Z = 0.25 * s
	}
}

// SetFromUnitVectors sets this QuaternionT[T] to the rotation from vector vFrom to vTo.
// The vectors must be normalized.
// Returns pointer to this updated QuaternionT[T].
func (q *QuaternionT[T]) SetFromUnitVectors(vFrom, vTo *Vec3T[T]) {

	var v1 Vec3T[T]

	r := vFrom.Dot(vTo) + 1
	if r < EPS {

		r = 0
		if Abs(vFrom.X) > Abs(vFrom.Z) {
			v1.Set(-vFrom.Y, vFrom.X, 0)
		} else {
			v1.Set(0, -vFrom.Z, vFrom.Y)
		}

	} else {
		v1 = vFrom.Cross(vTo)
	}
	q.X = v1.X
	q.Y = v1.Y
	q.Z = v1.Z
	q.W = r

	q.Normalize()

}

// Inverse sets this QuaternionT[T] to its inverse.
// Returns pointer to this updated QuaternionT[T].
func (q *QuaternionT[T]) Inverse() {

	q.Conjugate()
	q.Normalize()
}

// Conjugate sets this QuaternionT[T] to its conjugate.
// Returns pointer to this updated QuaternionT[T].
func (q *QuaternionT[T]) Conjugate() {

	q.X *= -1
	q.Y *= -1
	q.Z *= -1
}

// Dot returns the dot products of this QuaternionT[T] with other.
func (q *QuaternionT[T]) Dot(other *QuaternionT[T]) T {

	return q.X*other.X + q.Y*other.Y + q.Z*other.Z + q.W*other.W
}

// LengthSq returns this quanternion's length squared
func (q *QuaternionT[T]) lengthSq() T {

	return q.X*q.X + q.Y*q.Y + q.Z*q.Z + q.W*q.W
}

// Length returns the length of this QuaternionT[T]
func (q *QuaternionT[T]) Length() T {

	return Sqrt(q.X*q.X + q.Y*q.Y + q.Z*q.Z + q.W*q.W)
}

// Normalize normalizes this QuaternionT[T].
// Returns pointer to this updated QuaternionT[T].
func (q *QuaternionT[T]) Normalize() {

	l := q.Length()
	if l == 0 {
		q.X = 0
		q.Y = 0
		q.Z = 0
		q.W = 1
	} else {
		l = 1 / l
		q.X *= l
		q.Y *= l
		q.Z *= l
		q.W *= l
	}
}

// NormalizeFast approximates normalizing this QuaternionT[T].
// Works best when the QuaternionT[T] is already almost-normalized.
// Returns pointer to this updated QuaternionT[T].
func (q *QuaternionT[T]) NormalizeFast() {

	f := (3.0 - (q.X*q.X + q.Y*q.Y + q.Z*q.Z + q.W*q.W)) / 2.0
	if f == 0 {
		q.X = 0
		q.Y = 0
		q.Z = 0
		q.W = 1
	} else {
		q.X *= f
		q.Y *= f
		q.Z *= f
		q.W *= f
	}
}

// Multiply sets this QuaternionT[T] to the multiplication of itself by other.
// Returns pointer to this updated QuaternionT[T].
func (q *QuaternionT[T]) Multiply(other *QuaternionT[T]) *QuaternionT[T] {

	return q.MultiplyQuaternionTs(q, other)
}

// MultiplyQuaternionT[T]s set this QuaternionT[T] to the multiplication of a by b.
// Returns pointer to this updated QuaternionT[T].
func (q *QuaternionT[T]) MultiplyQuaternionTs(a, b *QuaternionT[T]) *QuaternionT[T] {

	// from http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/QuaternionT[T]s/code/index.htm

	qax := a.X
	qay := a.Y
	qaz := a.Z
	qaw := a.W
	qbx := b.X
	qby := b.Y
	qbz := b.Z
	qbw := b.W

	q.X = qax*qbw + qaw*qbx + qay*qbz - qaz*qby
	q.Y = qay*qbw + qaw*qby + qaz*qbx - qax*qbz
	q.Z = qaz*qbw + qaw*qbz + qax*qby - qay*qbx
	q.W = qaw*qbw - qax*qbx - qay*qby - qaz*qbz
	return q
}

// Slerp sets this QuaternionT[T] to another QuaternionT[T] which is the spherically linear interpolation
// from this QuaternionT[T] to other using t.
// Returns pointer to this updated QuaternionT[T].
func (q *QuaternionT[T]) Slerp(other *QuaternionT[T], t T) {

	if t == 0 {
		return
	}
	if t == 1 {
		q.CopyFrom(other)
		return
	}

	x := q.X
	y := q.Y
	z := q.Z
	w := q.W

	cosHalfTheta := w*other.W + x*other.X + y*other.Y + z*other.Z

	if cosHalfTheta < 0 {
		q.W = -other.W
		q.X = -other.X
		q.Y = -other.Y
		q.Z = -other.Z
		cosHalfTheta = -cosHalfTheta
	} else {
		q.CopyFrom(other)
	}

	if cosHalfTheta >= 1.0 {
		q.W = w
		q.X = x
		q.Y = y
		q.Z = z
		return
	}

	sqrSinHalfTheta := 1.0 - cosHalfTheta*cosHalfTheta
	if sqrSinHalfTheta < 0.001 {
		s := 1 - t
		q.W = s*w + t*q.W
		q.X = s*x + t*q.X
		q.Y = s*y + t*q.Y
		q.Z = s*z + t*q.Z
		q.Normalize()
		return
	}

	sinHalfTheta := Sqrt(sqrSinHalfTheta)
	halfTheta := Atan2(sinHalfTheta, cosHalfTheta)
	ratioA := Sin((1-t)*halfTheta) / sinHalfTheta
	ratioB := Sin(t*halfTheta) / sinHalfTheta

	q.W = w*ratioA + q.W*ratioB
	q.X = x*ratioA + q.X*ratioB
	q.Y = y*ratioA + q.Y*ratioB
	q.Z = z*ratioA + q.Z*ratioB

}

// Equals returns if this QuaternionT[T] is equal to other.
func (q *QuaternionT[T]) Equals(other *QuaternionT[T]) bool {

	return (other.X == q.X) && (other.Y == q.Y) && (other.Z == q.Z) && (other.W == q.W)
}

// FromArray sets this QuaternionT[T]'s components from array starting at offset.
// Returns pointer to this updated QuaternionT[T].
func (q *QuaternionT[T]) FromArray(array []T, offset int) {

	q.X = array[offset]
	q.Y = array[offset+1]
	q.Z = array[offset+2]
	q.W = array[offset+3]
}

// ToArray copies this QuaternionT[T]s's components to array starting at offset.
// Returns pointer to this updated array.
func (q *QuaternionT[T]) ToArray(array []T, offset int) []T {

	array[offset] = q.X
	array[offset+1] = q.Y
	array[offset+2] = q.Z
	array[offset+3] = q.W

	return array
}
